#!/usr/bin/env python3
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
from launch_ros.substitutions import FindPackageShare
import yaml
import os
import json
import logging

logger = logging.getLogger("terminal_launch_logger")

def GetConfig():
    """读取并解析USV配置文件"""
    current_path = os.path.abspath('.')
    yaml_path = os.path.join(current_path, "PlanningNode/usv_plan.yaml")
    
    if not os.path.exists(yaml_path):
        logger.error(f"配置文件未找到: {yaml_path}")
        raise FileNotFoundError(f"配置文件未找到: {yaml_path}")
    
    try:
        with open(yaml_path, 'r', encoding='utf-8') as file:
            yaml_content = ''.join(line for line in file if not line.strip().startswith('#'))
            return yaml.safe_load(yaml_content)
    except Exception as e:
        logger.error(f"配置文件解析失败: {str(e)}")
        raise

def generate_launch_description():
    launchDesc = LaunchDescription()
    config_data = GetConfig()
    
    # 验证基本结构
    if 'types' not in config_data:
        print("错误: 配置文件中缺少'ships'根节点")
        return []
    
    # 遍历所有舰船类型
    for type_idx, type_config in enumerate(config_data['types']):
        type_id = type_config.get('type', type_idx)
        ship_count = type_config['ship_count']
        ships = type_config.get('ships', [])
        # ship_count += len(ships)
        
        print(f"类型 {type_id}: {len(ships)} 艘船")
        ship_count = min(ship_count, len(ships))  # 取最小值避免越界
        
        for idx in range(ship_count):
            ship = ships[idx]
            ship_id = idx  # 类别内编号：0,1,2,...

        # 为每艘船创建节点
        # for ship_idx, ship in enumerate(ships):
        #     ship_id = ship.get('id', ship_idx)
            
            if ship_id>= ship_count:
                print(f"设置船数 {ship_count}, 小于保存船数{len(ships)}")
                break
            # 只提取该艘船的配置
            ship_config = {
                'type': type_id,
                'id': ship_id,
                'waypoint_count': len(ship.get('waypoints', [])),
                'waypoints': ship.get('waypoints', [])
            }
            
            # 生成唯一的节点名称
            node_name = f"plan_{type_id}_{ship_id}"
            
            # 将配置转换为JSON字符串
            ship_config_json = json.dumps(ship_config)
            
            # 创建节点
            ship_node = Node(
                package=config_data['package'],
                executable=config_data['executable'],
                name=node_name,
                parameters=[
                    {'ship_config': ship_config_json}
                ],
                output=config_data['output']
            )
            launchDesc.add_action(ship_node)
            # launchDesc.append(ship_node)
            
            print(f"  🚢 船 {ship_id}: 配置长度 {len(ship_config_json)} 字节")
    
    # print(f"共计创建 {ship_count} 个船节点")
    
    return launchDesc